纯定位模式 (一) | 沉默杀手

纯定位时载入的地图在submap中的trajectory为0，启动定位后，系统默认从起点开始建立子图进行匹配，该子图在submap中的trajectory为 1。

一条trajectory可以理解为一次建图过程。如果只让机器人跑一圈，那么trajectory数就是1. 但是，建好图后又需要在图中走，这时候可以增加一条trajectory，把pure_localization设为true，那么机器人再重新跑的过程中就会跟已经建好的图进行匹配，估计机器人在地图中的路径。所以，一次运行就代表了一条trajectory。

cartographer定位时，它的地图是变化的

纯定位和建图不同，local和global SLAM之间的延迟要尽量低. 其次global SLAM通常会在作为地图的frozen trajectory和当前trajectory之间，找到大量的相互约束(inter constraints)。

纯定位模式默认只保存 3 个子图，只有这些子图内的节点参与优化。所以地图大小并不会影响后端优化的速度。而建图结束的时候全图优化应该是所有节点参与。

我们需要显著减小POSE_GRAPH.optimize_every_n_nodes以接收频繁的结果， global SLAM通常会太慢，所以再显著减小global_sampling_ratio和constraint_builder.sampling_ratio以补偿大量的约束。

submaps.resolution应当和运行的.pbstream中的子图resolution一致

建图时，cartographer使用 fast correlative scan matcher找到回环，search window默认为7m×7m×30°. 但纯定位模式中，search window变得非常大，所以纯定位消耗CPU更大

constraint_builder_2d.cc中的ComputeConstraint()函数，在纯定位模式下，会频繁地调用MaybeAddGlobalConstraint()，这个函数会搜索整个子图，造成的开销特别大。建议将global_sampling_ratio调小，这样就可以从一定程度上减少开销。

数据集的纯定位测试

程序运行时只在局部建图（保留固定数量的子图），随着机器人的运动和观测，新的子图会添加进来，旧的子图会被删除，就是只保留局部的地图数据。我的理解是保留局部数据越多，与原有地图进行回环检测就增多，这样计算量增加的同时也增加了误匹配的概率，使得整个系统不稳定，所以这里只进行定位，不用于建图

需要用到数据集b2-2016-04-05-14-44-52.bag和b2-2016-04-27-12-31-41.bag，前者用于建图，后者用于定位

先运行建图：

1	roslaunch cartographer_ros offline_backpack_2d.launch bag_filenames:=${HOME}/path/b2-2016-04-05-14-44-52.bag

等一会，完成后会生成pbstream文件，如果运行建图，实际地图是这样的
建图.png

这里是先用第一个数据集建图，建完图后用第二个数据集进行定位测试。

1	roslaunch cartographer_ros demo_backpack_2d_localization.launch load_state_filename:=${BAG}/b2-2016-04-05-14-44-52.bag.pbstream bag_filename:=${BAG}/b2-2016-04-27-12-31-41.bag

后面两个参数一个是上一步建好的地图，pbstream格式，另一个是数据集。你会看到机器人很快实现了定位

在机器人的旧版本纯定位

用于定位时的home_no_odom_localization.launch

<launch>
  <node name="cartographer_node" pkg="cartographer_ros"
      type="cartographer_node" args="
          -configuration_directory $(find cartographer_ros)/configuration_files
          -configuration_basename home_no_odom_localization.lua
          -load_state_filename /home/xiaoqiang/Documents/ros/maps/home.pbstream"
      output="screen">
  </node>

  <node name="cartographer_occupancy_grid_node" pkg="cartographer_ros"
      type="cartographer_occupancy_grid_node" args="-resolution 0.05" />
</launch>

使用cartographer建图和定位时，launch文件的不同之处在于cartographer_node节点的参数-configuration_basename和-load_state_filename，前者就是lua文件的不同，后者只在定位时才有，毕竟没有地图就不能定位

建图使用no_odom_and_imu.lua，定位使用home_no_odom_localization.lua:

include "home.lua"

TRAJECTORY_BUILDER.pure_localization = true
--- 每10个有效帧一个子图，子图构建完成要闭环检测一次，这个数越小，闭环检测越频繁，当然CPU增大
POSE_GRAPH.optimize_every_n_nodes = 5
POSE_GRAPH.constraint_builder.sampling_ratio = 0.1
POSE_GRAPH.global_sampling_ratio = 0.002

启动roslaunch cartographer_ros home_localization.launch后应该就可以了，必须有map坐标系，否则就是没成功。
定位结果.png

optimize_every_n_nodes会影响定位时的CPU占用，设置为5时对应50%，但只有定位时的一秒左右，所以无所谓。

添加轨迹实际调用的源码是node.cc中的Node::HandleStartTrajectory——AddTrajectory

新版本的配置

这是因为我的 cartographer版本旧，不支持pure_localization_trimmer，最好还是更新到最新。在18年7月增加了pure_localization_trimmer，删掉了pure_localization，如果是新一点的版本，做对应修改。commit历史在这里

相应的源码在 MapBuilder::AddTrajectoryBuilder——MaybeAddPureLocalizationTrimmer：

if (trajectory_options.pure_localization()) 
{
  LOG(WARNING) << "'TrajectoryBuilderOptions::pure_localization' field is deprecated"
         "Use 'TrajectoryBuilderOptions::pure_localization_trimmer' instead.";
  pose_graph->AddTrimmer(absl::make_unique<PureLocalizationTrimmer>(
      trajectory_id, 3 /* max_submaps_to_keep */)  );
  return;
}
if (trajectory_options.has_pure_localization_trimmer()) 
{
  // 添加一个PureLocalizationTrimmer类型修饰器
  pose_graph->AddTrimmer(absl::make_unique<PureLocalizationTrimmer>(
      trajectory_id,
      trajectory_options.pure_localization_trimmer().max_submaps_to_keep() ) );
}

新版本的配置:

TRAJECTORY_BUILDER.pure_localization_trimmer = {
  --- 最大保存子图数，存定位模式通过子图进行定位，但只需要当前和上一个子图即可
  max_submaps_to_keep = 3,
}

如果使用了机器人的odom，但无法定位成功或定位误差太大，可能的原因是odom不够精确，尝试关闭odom，并由cartographer代为提供odom。既然定位误差大，可能在建图时的原始submap之间误差较大，图优化后map的边缘较为模糊

重定位注释掉Node::GrawAndPublish函数的后三行，否则会继续建图。但是建图的时候还得加回来。
由于Cartographer定位是当前帧和子图进行匹配，所以对于机器人周围被新物体遮挡的情况完全没有影响。

唯一影响就是没法重定位了，因为地图上没有这些新物体。但是就算长时间被遮挡，存在累积定位误差，只要机器人离开了遮挡区域，回到了之前的环境，就可以重定位回来。

获得机器人在map坐标系中的坐标

cartographer是使用tf转换来发布位姿的，如果想发布类似amcl_pose那样的话题，只能修改源码或自己写个程序。

修改源码的方法可以参考这里

我还是自己写了个程序，其实就是tf报警：Lookup would require extrapolation into the past中的程序改了坐标系和话题名称，以获得base_footprint在map中的坐标系

问题

某次运行得到这样结果：

追查到node.cc里的Node::FinishTrajectoryUnderLock函数，检查了一下是否可以关掉，指定id是否存在，是否已经被Finished了等情况后，如果一切正常，则停止订阅Topic、清除id及其他与该trajectory相关的量。最后调用map_builder_bridge_中的FinishTrajectory函数。发现code对应cartographer_ros_msgs::StatusCode::INVALID_ARGUMENT

参考：
官方参考
 更优雅的设定初始位姿